home *** CD-ROM | disk | FTP | other *** search
/ Graphics Plus / Graphics Plus.iso / libs / sphigs / sph_mac.hqx / SRGP port to 5.0 (compressed) / SRGP_SPHIGS Root / MacSPHIGS / MAT3inv.c < prev    next >
Encoding:
C/C++ Source or Header  |  1992-02-13  |  8.8 KB  |  318 lines
  1. /* Copyright 1988, Brown Computer Graphics Group.  All Rights Reserved. */
  2.  
  3. /* --------------------------------------------------------------------------
  4.  * This file contains routines that operate solely on matrices.
  5.  * -------------------------------------------------------------------------*/
  6.  
  7. #include "mat3defs.h"
  8.  
  9. /* --------------------------  Static Routines    ---------------------------- */
  10.  
  11. #define SMALL 1e-20        /* Small enough to be considered zero */
  12.  
  13. #include "MAT3inv.proto.h"
  14. static void MAT3_inv3_swap(register double[3 ][3 ], int, int, int);
  15. static int MAT3_inv3_second_col(register double[3 ][3 ], register double[3 ][3 ], int);
  16. static int MAT3_invert3(register double[3 ][3 ], register double[3 ][3 ]);
  17. static int MAT3_inv4_pivot(register MAT3mat, MAT3vec, double *, int *);
  18.  
  19. /*
  20.  * Shuffles rows in inverse of 3x3.  See comment in MAT3_inv3_second_col().
  21.  */
  22.  
  23. static void
  24. MAT3_inv3_swap( register double inv[3][3], int row0, int row1, int row2)
  25. {
  26.    register int i, tempi;
  27.    double    temp;
  28.  
  29. #define SWAP_ROWS(a, b) \
  30.    for (i = 0; i < 3; i++) SWAP(inv[a][i], inv[b][i], temp); \
  31.    SWAP(a, b, tempi)
  32.  
  33.    if (row0 != 0){
  34.       if (row1 == 0) {
  35.      SWAP_ROWS(row0, row1);
  36.       }
  37.       else {
  38.      SWAP_ROWS(row0, row2);
  39.       }
  40.    }
  41.  
  42.    if (row1 != 1) {
  43.       SWAP_ROWS(row1, row2);
  44.    }
  45. }
  46.  
  47. /*
  48.  * Does Gaussian elimination on second column.
  49.  */
  50.  
  51. static int
  52. MAT3_inv3_second_col (register double source[3][3], register double inv[3][3], int row0)
  53. {
  54.    register int row1, row2, i1, i2, i;
  55.    double    temp;
  56.    double    a, b;
  57.  
  58.    /* Find which row to use */
  59.    if       (row0 == 0)    i1 = 1, i2 = 2;
  60.    else if (row0 == 1)    i1 = 0, i2 = 2;
  61.    else         i1 = 0, i2 = 1;
  62.  
  63.    /* Find which is larger in abs. val.:the entry in [i1][1] or [i2][1]    */
  64.    /* and use that value for pivoting.                    */
  65.  
  66.    a = source[i1][1]; if (a < 0) a = -a;
  67.    b = source[i2][1]; if (b < 0) b = -b;
  68.    if (a > b)     row1 = i1;
  69.    else        row1 = i2;
  70.    row2 = (row1 == i1 ? i2 : i1);
  71.  
  72.    /* Scale row1 in source */
  73.    if ((source[row1][1] < SMALL) && (source[row1][1] > -SMALL)) return(FALSE);
  74.    temp = 1.0 / source[row1][1];
  75.    source[row1][1]  = 1.0;
  76.    source[row1][2] *= temp;    /* source[row1][0] is zero already */
  77.  
  78.    /* Scale row1 in inv */
  79.    inv[row1][row1]  = temp;    /* it used to be a 1.0 */
  80.    inv[row1][row0] *= temp;
  81.  
  82.    /* Clear column one, source, and make corresponding changes in inv */
  83.  
  84.    for (i = 0; i < 3; i++) if (i != row1) {    /* for i = all rows but row1 */
  85.    temp = -source[i][1];
  86.       source[i][1]  = 0.0;
  87.       source[i][2] += temp * source[row1][2];
  88.  
  89.       inv[i][row1]  = temp * inv[row1][row1];
  90.       inv[i][row0] += temp * inv[row1][row0];
  91.    }
  92.  
  93.    /* Scale row2 in source */
  94.    if ((source[row2][2] < SMALL) && (source[row2][2] > -SMALL)) return(FALSE);
  95.    temp = 1.0 / source[row2][2];
  96.    source[row2][2] = 1.0;    /* source[row2][*] is zero already */
  97.  
  98.    /* Scale row2 in inv */
  99.    inv[row2][row2]  = temp;    /* it used to be a 1.0 */
  100.    inv[row2][row0] *= temp;
  101.    inv[row2][row1] *= temp;
  102.  
  103.    /* Clear column one, source, and make corresponding changes in inv */
  104.    for (i = 0; i < 3; i++) if (i != row2) {    /* for i = all rows but row2 */
  105.    temp = -source[i][2];
  106.       source[i][2] = 0.0;
  107.       inv[i][row0] += temp * inv[row2][row0];
  108.       inv[i][row1] += temp * inv[row2][row1];
  109.       inv[i][row2] += temp * inv[row2][row2];
  110.    }
  111.  
  112.    /*
  113.     * Now all is done except that the inverse needs to have its rows shuffled.
  114.     * row0 needs to be moved to inv[0][*], row1 to inv[1][*], etc.
  115.     *
  116.     * We *didn't* do the swapping before the elimination so that we could more
  117.     * easily keep track of what ops are needed to be done in the inverse.
  118.     */
  119.    MAT3_inv3_swap(inv, row0, row1, row2);
  120.  
  121.    return(TRUE);
  122. }
  123.  
  124. /*
  125.  * Fast inversion routine for 3 x 3 matrices.    - Written by jfh.
  126.  *
  127.  * This takes 30 multiplies/divides, as opposed to 39 for Cramer's Rule.
  128.  * The algorithm consists of performing fast gaussian elimination, by never
  129.  * doing any operations where the result is guaranteed to be zero, or where
  130.  * one operand is guaranteed to be zero. This is done at the cost of clarity,
  131.  * alas.
  132.  *
  133.  * Returns 1 if the inverse was successful, 0 if it failed.
  134.  */
  135.  
  136. static int
  137. MAT3_invert3 (register double source[3][3], register double inv[3][3])
  138. {
  139.    register int i, row0;
  140.    double    temp;
  141.    double    a, b, c;
  142.  
  143.    inv[0][0] = inv[1][1] = inv[2][2] = 1.0;
  144.    inv[0][1] = inv[0][2] = inv[1][0] = inv[1][2] = inv[2][0] = inv[2][1] = 0.0;
  145.  
  146.    /* attempt to find the largest entry in first column to use as pivot */
  147.    a = source[0][0]; if (a < 0) a = -a;
  148.    b = source[1][0]; if (b < 0) b = -b;
  149.    c = source[2][0]; if (c < 0) c = -c;
  150.  
  151.    if (a > b) {
  152.       if (a > c) row0 = 0;
  153.       else row0 = 2;
  154.    }
  155.    else { 
  156.       if (b > c) row0 = 1;
  157.       else row0 = 2;
  158.    }
  159.  
  160.    /* Scale row0 of source */
  161.    if ((source[row0][0] < SMALL) && (source[row0][0] > -SMALL)) return(FALSE);
  162.    temp = 1.0 / source[row0][0];
  163.    source[row0][0]  = 1.0;
  164.    source[row0][1] *= temp;
  165.    source[row0][2] *= temp;
  166.  
  167.    /* Scale row0 of inverse */
  168.    inv[row0][row0] = temp;    /* other entries are zero -- no effort    */
  169.  
  170.    /* Clear column zero of source, and make corresponding changes in inverse */
  171.  
  172.    for (i = 0; i < 3; i++) if (i != row0) {    /* for i = all rows but row0 */
  173.       temp = -source[i][0];
  174.       source[i][0]  = 0.0;
  175.       source[i][1] += temp * source[row0][1];
  176.       source[i][2] += temp * source[row0][2];
  177.       inv[i][row0]  = temp * inv[row0][row0];
  178.    }
  179.  
  180.    /*
  181.     * We've now done gaussian elimination so that the source and
  182.     * inverse look like this:
  183.     *
  184.     *    1 * *        * 0 0
  185.     *    0 * *        * 1 0
  186.     *    0 * *        * 0 1
  187.     *
  188.     * We now proceed to do elimination on the second column.
  189.     */
  190.    if (! MAT3_inv3_second_col(source, inv, row0)) return(FALSE);
  191.  
  192.    return(TRUE);
  193. }
  194.  
  195. /*
  196.  * Finds a new pivot for a non-simple 4x4.  See comments in MAT3invert().
  197.  */
  198.  
  199. static int
  200. MAT3_inv4_pivot (register MAT3mat src, MAT3vec r, double *s, int *swap)
  201. {
  202.    register int i, j;
  203.    double    temp, max;
  204.  
  205.    *swap = -1;
  206.  
  207.    if (MAT3_IS_ZERO(src[3][3])) {
  208.  
  209.       /* Look for a different pivot element: one with largest abs value */
  210.       max = 0.0;
  211.  
  212.       for (i = 0; i < 4; i++) {
  213.      if     (src[i][3] >  max) max =  src[*swap = i][3];
  214.      else if (src[i][3] < -max) max = -src[*swap = i][3];
  215.       }
  216.  
  217.       /* No pivot element available ! */
  218.       if (*swap < 0) return(FALSE);
  219.  
  220.       else for (j = 0; j < 4; j++) SWAP(src[*swap][j], src[3][j], temp);
  221.    }
  222.  
  223.    MAT3_SET_VEC (r, -src[0][3], -src[1][3], -src[2][3]);
  224.  
  225.    *s = 1.0 / src[3][3];
  226.  
  227.    src[0][3] = src[1][3] = src[2][3] = 0.0;
  228.    src[3][3]                 = 1.0;
  229.  
  230.    MAT3_SCALE_VEC(src[3], src[3], *s);
  231.  
  232.    for (i = 0; i < 3; i++) {
  233.       src[0][i] += r[0] * src[3][i];
  234.       src[1][i] += r[1] * src[3][i];
  235.       src[2][i] += r[2] * src[3][i];
  236.    }
  237.  
  238.    return(TRUE);
  239. }
  240.  
  241. /* -------------------------  Internal Routines  --------------------------- */
  242.  
  243. /* --------------------------  Public Routines    ---------------------------- */
  244.  
  245. /*
  246.  * This returns the inverse of the given matrix.  The result matrix
  247.  * may be the same as the one to invert.
  248.  *
  249.  * Fast inversion routine for 4 x 4 matrices, written by jfh.
  250.  *
  251.  * Returns 1 if the inverse was successful, 0 if it failed.
  252.  *
  253.  * This routine has been specially tweaked to notice the following:
  254.  * If the matrix has the form
  255.  *      * * * 0
  256.  *      * * * 0
  257.  *      * * * 0
  258.  *      * * * 1
  259.  *
  260.  * (as do many matrices in graphics), then we compute the inverse of
  261.  * the upper left 3x3 matrix and use this to find the general inverse.
  262.  *
  263.  *   In the event that the right column is not 0-0-0-1, we do gaussian
  264.  * elimination to make it so, then use the 3x3 inverse, and then do
  265.  * our gaussian elimination.
  266.  */
  267.  
  268. int
  269. MAT3invert(MAT3mat result_mat, MAT3mat mat)
  270. {
  271.    MAT3mat        src, inv;
  272.    register int     i, j, simple;
  273.    double        m[3][3], inv3[3][3], s, temp;
  274.    MAT3vec        r, t;
  275.    int            swap;
  276.  
  277.    MAT3copy(src, mat);
  278.    MAT3identity(inv);
  279.  
  280.    /* If last column is not (0,0,0,1), use special code */
  281.    simple = (mat[0][3] == 0.0 && mat[1][3] == 0.0 &&
  282.          mat[2][3] == 0.0 && mat[3][3] == 1.0);
  283.  
  284.    if (! simple && ! MAT3_inv4_pivot(src, r, &s, &swap)) return(FALSE);
  285.  
  286.    MAT3_COPY_VEC(t, src[3]);    /* Translation vector */
  287.  
  288.    /* Copy upper-left 3x3 matrix */
  289.    for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) m[i][j] = src[i][j];
  290.  
  291.    if (! MAT3_invert3(m, inv3)) return(FALSE);
  292.  
  293.    for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) inv[i][j] = inv3[i][j];
  294.  
  295.    for (i = 0; i < 3; i++) for (j = 0; j < 3; j++)
  296.       inv[3][i] -= t[j] * inv3[j][i];
  297.  
  298.    if (! simple) {
  299.  
  300.       /* We still have to undo our gaussian elimination from earlier on */
  301.       /* add r0 * first col to last col */
  302.       /* add r1 * 2nd    col to last col */
  303.       /* add r2 * 3rd    col to last col */
  304.  
  305.       for (i = 0; i < 4; i++) {
  306.      inv[i][3] += r[0] * inv[i][0] + r[1] * inv[i][1] + r[2] * inv[i][2];
  307.      inv[i][3] *= s;
  308.       }
  309.  
  310.       if (swap >= 0)
  311.      for (i = 0; i < 4; i++) SWAP(inv[i][swap], inv[i][3], temp);
  312.    }
  313.  
  314.    MAT3copy(result_mat, inv);
  315.  
  316.    return(TRUE);
  317. }
  318.